#include "Keyboard_Ctrl_Node.h"
#include <iostream>
#include <ncurses.h>
#include <chrono>

using namespace std::chrono_literals;
rclcpp::Logger Keyboard_Ctrl_Node::TRACE = rclcpp::get_logger("Keyboard_Ctrl_Node");

Keyboard_Ctrl_Node::Keyboard_Ctrl_Node(const std::string& node_name) 
    : Node(node_name)
    , its_timer(create_wall_timer(100ms, std::bind(&Keyboard_Ctrl_Node::on_timer, this)))
    , its_ctrl_publisher(create_publisher<geometry_msgs::msg::Twist>("cmd_vel", 10))
{
    initscr();              // 初始化 ncurses 模式
    cbreak();               // 禁用行缓冲，传递所有控制信息
    noecho();               // 禁止回显
    nodelay(stdscr, TRUE);  // 设置 getch() 为非阻塞模式
}

Keyboard_Ctrl_Node::~Keyboard_Ctrl_Node()
{
    endwin();    // 结束 ncurses 模式
}

void Keyboard_Ctrl_Node::on_timer()
{
    int ch;
    std::vector<int> keys;
    keys.clear();

    // 检测所有被按下的键
    while ((ch = getch()) != ERR) {
        keys.push_back(ch);
    }

    char cmd = 0;
    if (!keys.empty()) 
    {
        cmd = keys[0];
        for (int key : keys) 
        {
            if(key != cmd)
            {
                cmd = 0;//不处理多个按键同时按下的情况
                break;
            }
        }
    }

    switch (cmd)
    {
    case 'w':
    case 'W':
        action_forward();
        break;
    
    case 'a':
    case 'A':
        action_left();
        break;

    case 's':
    case 'S':
        action_back();
        break;

    case 'd':
    case 'D':
        action_right();
        break;
    
    default:
        action_stop();
        break;
    }
}

void Keyboard_Ctrl_Node::action_forward()
{
    publish_ctrl_cmd(1.0, 0.0);
}

void Keyboard_Ctrl_Node::action_back()
{
    publish_ctrl_cmd(-1.0, 0.0);
}

void Keyboard_Ctrl_Node::action_left()
{
    publish_ctrl_cmd(1.0, 0.5);
}

void Keyboard_Ctrl_Node::action_right()
{
    publish_ctrl_cmd(1.0, -0.5);
}

void Keyboard_Ctrl_Node::action_stop()
{
    publish_ctrl_cmd(0.0, 0.0);
}


void Keyboard_Ctrl_Node::publish_ctrl_cmd(const double& line_x, const double& angle_z)
{
    geometry_msgs::msg::Twist msg;
    msg.linear.x = line_x;
    msg.linear.y = 0.0;
    msg.linear.z = 0.0;
    msg.angular.x = 0.0;
    msg.angular.y = 0.0;
    msg.angular.z = angle_z;

    its_ctrl_publisher->publish(msg);
}
